#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <rclcpp_components/register_node_macro.hpp>
#include "std_msgs/msg/string.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "geometry_msgs/msg/pose.hpp"

#include <iostream>

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include "rclcpp/time.hpp"
#include <rclcpp/time_source.hpp>

#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include <opencv2/highgui.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
{   
  private:
    std::string image_dir_;
    std::string pcd_dir_;
    std::string source_frame_;
    std::string target_frame_;

    std::vector<double> transform_;

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;

    sensor_msgs::msg::Image image_;
    sensor_msgs::msg::PointCloud2 pointcloud_;
    geometry_msgs::msg::TransformStamped tf_;

    rclcpp::TimerBase::SharedPtr timer_;

    size_t count_;  

    void timer_callback()
    { 
      // auto message = std_msgs::msg::String();
      // message.data = "Hello, world! " + std::to_string(count_++);
      // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      // publisher_->publish(message);

      tf_.header.stamp = this->now();
      static_tf_broadcaster->sendTransform(tf_);

      RCLCPP_INFO(this->get_logger(), "I send a image");
      image_.header.stamp = this->now();
      image_publisher_->publish(image_);

      RCLCPP_INFO(this->get_logger(), "I send a pointcloud");
      pointcloud_.header.stamp = this->now();
      pointcloud_publisher_->publish(pointcloud_);
    }

  public:
    MinimalPublisher()
    : Node("task_publisher"), count_(0)
    { 
      std::string dataset_dir = declare_parameter<string>("dataset_dir", "not declared");
      std::string image_name = declare_parameter<string>("image_name", "not declared");
      std::string pcd_name = declare_parameter<string>("pcd_name", "not declared");
      source_frame_ = declare_parameter<string>("source_frame", "not declared");
      target_frame_ = declare_parameter<string>("target_frame", "not declared");

      transform_ = declare_parameter<vector<double>>("transform", {});
      image_dir_ = os.path.join(dataset_dir, image_name);
      pcd_dir_ = os.path.join(dataset_dir, pcd_name);

      // publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
      image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image_raw", 10);
      pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud_raw", 10);

      tf_.header.frame_id = source_frame_;
      tf_.child_frame_id = target_frame_;
      tf_.transform.translation.x = transform_[0];
      tf_.transform.translation.y = transform_[1];
      tf_.transform.translation.z = transform_[2];
      tf2::Quaternion q;
      q.setRPY(transform_[3], transform_[4], transform_[5]);
      tf_.transform.rotation = tf2::toMsg(q);

      cv::Mat img;
      img = cv::imread(image_dir_, cv::IMREAD_UNCHANGED);
      cv_bridge::CvImage cv_image;
      cv_image = cv_bridge::CvImage(image_.header, sensor_msgs::image_encodings::BGR8, img);
      cv_image.toImageMsg(image_);
      image_.header.frame_id = "base_link";

      pcl::PointCloud<pcl::PointXYZI>::Ptr map;
      map.reset(new pcl::PointCloud<pcl::PointXYZI>());
      pcl::io::loadPCDFile(pcd_dir_, *map);
      pcl::toROSMsg(*map, pointcloud_);
      pointcloud_.header.frame_id = "map";

      timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}